//
// Created by jm on 2021/5/28.
//

#ifndef DMP_GRAPH_SEARCH_H
#define DMP_GRAPH_SEARCH_H

#include <boost/heap/d_ary_heap.hpp>      // boost::heap::d_ary_heap
#include <memory>                         // std::shared_ptr
#include <limits>                         // std::numeric_limits
#include <vector>                         // std::vector
#include <unordered_map>                  // std::unordered_map
#include "util/data_type.h"
#include "p2p_util/data_utils.h"
#include <iostream>
namespace sz {
using Index = int;

///Define priority queue
struct DMPState; // forward declaration
///JPSState pointer
using DMPStatePtr = std::shared_ptr<DMPState>;
using DMPPriorityQueue = boost::heap::d_ary_heap<DMPStatePtr, boost::heap::mutable_<true>,
                                                 boost::heap::arity<2>, boost::heap::compare<compare_state<DMPStatePtr> >>;

///Node of the graph in graph search
struct DMPState {
  /// if has been opened
  bool opened{false};
  /// if has been closed
  bool closed{false};
  /// Coord
  short x{0}, y{0};
  /// ID
  Index id;
  /// id of predecessor
  Index parent_id{-1};

  /// pointer to heap location
  DMPPriorityQueue::handle_type heap_key;

  /// g cost
  float g{std::numeric_limits<float>::infinity()};
  /// heuristic cost
  float h{};

  /// 2D constructor
  DMPState(int id, short x, short y)
      : x(x), y(y) , id(id){
  }

};

/**
 * @brief JPSGraphSearch class
 *
 * Implement A* and Jump Point Search
 */
class DMPGraphSearch {
 public:
  /**
    * @brief 2D graph search constructor
    *
    * @param cMap 1D array stores the occupancy, with the order equal to \f$x + xDim * y\f$
    * @param xDim zslam_map length
    * @param yDim zslam_map width
    * @param eps weight of heuristic, optional, default as 1
    * @param cweight weight of distance cost, optional, default as 0.1
    * @param verbose flag for printing debug Info, optional, default as false
    */
  DMPGraphSearch(const uint8_t *cMap, short xDim, short yDim, float eps = 1, float cweight = 0.1, bool verbose = false);

  /**
   * @brief start 2D planning thread
   *
   * @param xStart start x coordinate
   * @param yStart start y coordinate
   * @param xGoal goal x coordinate
   * @param yGoal goal y coordinate
   * @param in_region a region that is valid for searching, empty means no boundary
   *
   * return the total path cost, which is infinity if failed
   */
  float Plan(short xStart, short yStart, short xGoal, short yGoal, const std::vector<bool> &in_region);

  /// Get the optimal path
  VectorPoint2i GetPath() const;

  /// Get the states in open set
  std::vector<DMPStatePtr> GetOpenSet() const;

  /// Get the states in close set
  std::vector<DMPStatePtr> GetCloseSet() const;

  /// Get the states in hash zslam_map
  std::vector<DMPStatePtr> GetAllSet() const;

 private:
  /// Main planning loop
  float Plan(DMPStatePtr &currNode_ptr, Index start_id, Index goal_id);
  /// Get successor function for A*
  void GetSucc(const DMPStatePtr &curr, std::vector<Index> &succ_ids, std::vector<float> &succ_costs);
  /// Recover the optimal path
  void RecoverPath(DMPStatePtr node, Index start_id);

  /// Get subscript
  int CoordToId(short x, short y) const;

  /// Check if (x, y) is free
  bool IsFree(short x, short y) const;

  /// Compute heuristic
  float GetHeur(short x, short y) const;

  bool verbose_;
  bool global_{};
  //const int8_t val_free_ = 0;
  const int8_t val_occ_ = 100;
  short xDim_, yDim_{};
  short xGoal_{}, yGoal_{};
  const uint8_t *cMap_;
  /// weight of heuristic
  float eps_;
  /// weight of distance zslam_map
  float dis_map_weight_;

  DMPPriorityQueue pq_;
  std::vector<DMPStatePtr> hm_;
  std::vector<bool> seen_;
  std::vector<bool> in_region_;
  VectorPoint2i path_;
  std::vector<std::vector<short>> ns_;
};
}
#endif
